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ABSTRACT 

There  are  two  goals  for  autonomous  vehicle  navigation  planning:  shortest 
path  and  safe  path.  These  goals  are  often  in  conflict;  path  safety  is  more 
important.  Safety  of  autonomous  vehicle  navigation  is  determined  by  the 
clearance  between  the  vehicle  and  obstacles.  Because  a  Voronoi  boundary  is  the 
set  of  points  locally  maximizing  the  clearance  from  obstacles-,  safety  is  maximized 
on  it.  Therefore,  Voronoi  Diagrams  are  suitable  for  motion  planning  of 
autonomous  vehicles. 

We  use  the  derivative  of  curvature  k  of  the  vehicle  motion  (dic/ds)  as  the 
only  control  variable  for  the  vehicle,  where  s  is  the  length  along  the  vehicle 
trajectory.  Previous  motion  planning  of  the  autonomous  mobile  robot  Yamabico- 
11  at  the  Naval  Postgraduate  School  used  a  path  tracking  method.  Before  the 
mission  began  the  vehicle  was  given  a  track  to  follow;  motion  planning  consisted 
of  calculating  the  point  on  the  track  closest  to  the  vehicle  and  calculating  dic/ds 
then  steering  the  vehicle  to  get  onto  the  track. 

We  propose  a  method  of  planning  safe  motions  of  the  vehicle  to  calculate 
optimal  dK/ds  at  each  point  directly  from  the  information  of  the  world  without 
calculating  the  track  to  follow.  This  safe  navigation  algorithm  is  fundamentally 
different  from  path  tracking  using  a  path  specification.  Additionally,  motion 
planning  is  simpler  and  faster  than  the  path  tracking  method. 

The  effectiveness  of  this  steering  function  for  vehicle  motion  control  is 
demonstrated  by  algorithmic  simulation  and  by  use  on  the  autonomous  mobile 
robot  Yamabico  11  at  the  Naval  Postgraduate  School. 
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THESIS  DISCLAIMER 

The  reader  is  cautioned  that  computer  programs  developed  in  this  research 
may  not  have  been  exercised  for  all  cases  of  interest.  While  every  effort  has  been 
made,  within  the  time  available,  to  ensure  that  the  programs  are  free  of 
computational  and  logic  errors,  they  cannot  be  considered  validated.  Any 
application  of  these  programs  without  additional  verification  is  at  the  risk  of  the 
user. 
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EXECUTIVE  SUMMARY 

There  are  two  goals  for  autonomous  vehicle  navigation  planning:  shortest 
path  and  safe  path.  These  goals  are  often  in  conflict;  path  safety  is  more 
important.  Safety  of  autonomous  vehicle  navigation  is  determined  by  the 
clearances  between  the  vehicle  and  obstacles.  Because  a  Voronoi  boundary  is  the 
set  of  points  locally  maximizing  the  clearance  from  obstacles,  safety  is  maximized 
on  it.  Therefore,  Voronoi  Diagrams  are  suitable  for  motion  planning  of 
autonomous  vehicles. 

We  use  the  derivative  of  curvature  k  of  the  vehicle  motion  (dK/ds)  as  the 
only  control  variable  for  the  vehicle,  where  s  is  the  length  along  the  vehicle 
trajectory.  Previous  motion  planning  of  the  autonomous  mobile  robot  Yamabico- 
11  at  the  Naval  Postgraduate  School  used  a  path  tracking  method.  Before  the 
mission  began  the  vehicle  was  given  a  track  to  follow;  motion  planning  consisted 
of  calculating  the  point  on  the  track  closest  to  the  vehicle  and  calculating  dK/ds 
then  steering  the  vehicle  to  get  onto  the  track.  The  safest  path  through  the  world 
navigated  by  the  vehicle  is  the  set  of  points  locally  maximizing  the  clearance  .from 
obstacles.  This  path  is  represented  by  the  Voronoi  diagram.  To  achieve  the  path 
tracking  method  it  is  necessary  to  calculate  the  Voronoi  boundary  which  consists 
of  line  segments  and  parabolic  arcs.  If  the  world  is  large,  it  is  complicated  and 
inefficient  to  calculate  every  Voronoi  boundary  of  this  world.  It  is  better  to 
calculate  optimal  dK/ds  at  each  point  directly  from  the  information  of  the  world 
without  calculating  the  track  to  follow. 

When  the  objects  are  two  points,  two  lines,  or  one  point  and  one  line,  we  can 
safely   navigate  the  vehicle  to  achieve  equal  clearances  from  these  objects.  The 
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motion  of  the  vehicle  is  optimized  at  each  point  directly  from  the  information  of 
the  obstacles  near  the  vehicle.  After  calculating  dic/ds,  the  vehicle  follows  the 
Voronoi  boundaries  defined  by  the  environment. 

Unlike  the  path  tracking  method,  this  method  can  be  applied  to  avoid  moving 
objects  since  we  calculate  the  optimal  motion  of  the  vehicle  at  each  point  directly 
from  the  information  of  the  world.  Additionally,  motion  control  is  simpler  and 
faster  than  in  the  path  tracking  method. 

The  effectiveness  of  this  steering  function  for  vehicle  motion  control  is 
demonstrated  by  algorithmic  simulation  and  by  use  on  the  autonomous  mobile 
robot  Yamabico  11  at  the  Naval  Postgraduate  School.  It  has  precise  knowledge  of 
its  location  in  a  given  environment  using  its  sonar  system.  The  robot  is 
programmed  by  the  high  level  mobile  robot  language  called  MML  (Model-based 
Mobile  robot  Language)  written  in  the  C  language. 
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I.    INTRODUCTION 

A.  BACKGROUND 

There  are  two  goals  for  planning  autonomous  vehicle  navigation  planning: 
shortest  path  and  safe  path.  These  goals  are  often  in  conflict;  path  safety  is  more 
important.  Safety  of  autonomous  vehicle  navigation  is  determined  by  the 
clearance  between  the  vc  cle  and  obstacles.  Because  a  Voronoi  boundary  is  the 
set  of  points  locally  maximizing  the  clearance  from  obstacles,  safety  is  maximized 
on  it.  Therefore,  Voronoi  Diagrams  are  suitable  for  motion  planning  of 
autonomous  vehicles. 

B.  OVERVIEW 

We  use  the  derivative  of  curvature  k  of  the  vehicle  motion  (dK/as)  as  the 
only  control  variable  for  the  vehicle,  where  s  is  the  length  along  the  vehicle 
trajectory.  Previous  motion  planning  of  the  autonomous  mobile  robot  Yamabico- 
11  at  the  Naval  Postgraduate  School  used  a  path  tracking  method  [Ref.  2].  Before 
the  mission  began  the  vehicle  was  given  a  track  to  follow;  motion  planning 
consisted  of  calculating  the  point  on  the  track  closest  to  the  vehicle  and 
calculating  dK/ds  then  steering  the  vehicle  to  get  onto  the  track.  The  safest  path 
through  the  world  navigated  by  the  vehicle  is  the  set  of  points  locally  maximizing 
the  clearance  from  obstacles.  This  path  is  represented  by  the  Voronoi  diagram.  To 
achieve  the  path  tracking  method  it  is  necessary  to  calculate  the  Voronoi 
boundary  which  consists  of  line  segments  and  parabolic  arcs.  If  the  world  is  large, 
it  is  complicated  and  inefficient  to  calculate  every  Voronoi  boundary  of  this 


world.  It  is  better  to  calculate  optimal  dKJds  at  each  point  directly  from  the 
information  of  the  world  without  calculating  the  track  to  follow. 

This  safe  navigation  algorithm  is  fundamentally  different  from  path  tracking 
using  a  path  specification.  Additionally,  motion  planning  is  simpler  and  faster  than 
in  the  path  tracking  method. 

The  effectiveness  of  this  steering  function  for  vehicle  motion  control  is 
demonstrated  by  algorithmic  simulation  and  by  use  on  the  autonomous  mobile 
robot  Yamabico  11  at  the  Naval  Postgraduate  School.  It  has  precise  knowledge  of 
its  location  in  a  given  environment  using  its  sonar  system.  The  robot  is 
programmed  by  the  high  level  mobile  robot  language  called  MML  (Model-based 
Mobile  robot  Language)  written  in  the  C  language  [Ref.  3]. 


II.  PROBLEM  STATEMENT 

The  problems  addressed  are  as  follows: 

1 .  How  to  navigate  the  robot  safely  to  achieve  the  same  clearance  from  two 
points. 

2.  How  to  navigate  the  robot  safely  to  achieve  the  same  clearance  from  two 
lines. 

3.  How  to  navigate  the  robot  safely  to  achieve  the  same  clean  e  from  one 
point  and  one  line. 

4.  How  to  execute  the  safest  path  by  a  real  robot. 

Safety  is  one  of  the  important  attributes  for  autonomous  vehicle  navigation 
planning.  Safety  is  determined  by  the  clearance  between  the  vehicle  and  objects. 
Assume  the  vehicle  is  a  point  object  and  W(p)  is  the  clearance  of  the  point  p.  The 
clearance  of  a  path  represents  its  safety.  If  the  clearance  is  small,  the  path  is 
dangerous  because  it  is  close  to  the  object  and  if  it  is  larger,  the  path  is  safer.  The 

clearance  of  a  path  n  is  defined  as: 

W(Il)  =  MinW(P)  (2.1) 

PeTl 

Where  n  is  the  path  of  the  vehicle  from  start  S  to  goal  G. 

We  want  to  find  the  path  no  such  that  W(U0)  is  the  maximum  among  all 

possible  paths  (Figure  2.1).  To  maximize  the  clearance  W(n),  we  will  take  the 

Voronoi  boundary  as  the  path  of  the  vehicle. 


Figure  2.1 :  Safety  Path 


III.    VORONOI  DIA    RAM 

A.  DEFINITIONS 

Assume  that  there  is  an  object  o  in  a  plane.  It  might  be  a  point,  a  line,  a  line 
segment,  a  polygon,  or  other  closed  sets  of  points.  If  a  world  W  has  more  than  one 
object,  a  Voronoi  region  of  an  object  oi  in  the  world  is  defined  as 

V(o()  =  [P\VJ[i  *  ;  ->  {dist(p,o,)<  dist(p,Oj)}]]  (3.1) 

Where  p  is  a  point  which  is  not  inside  ol  and  dist(p ,oJ  is  the  minimum 
distance  between  p  and  oi . 

The  set  of  all  the  Voronoi  regions  is  called  the  Voronoi  diagram  of  a  world. 
The  boundaries  of  Voronoi  regions  are  Voronoi  boundaries.  The  Voronoi 
diagram  of  a  geometric  world  typically  consists  of  lines,  rays,  line  segments,  and 
parabolic  arcs. 

B.  VORONOI  DIAGRAM  OF  TWO  POINTS 

In  the  case  that  a  world  consists  of  two  distinct  points,  pl  and  p2,  its  Voronoi 
boundary  is  the  bisector  of  those  two  points  which  generate  two  Voronoi  regions 
Vfa)  and  V(p2)  (Figure  3.1). 


Voronoi  boundary 

I  V(A) 

1 P 

V(p2) 
p2  =  \X2  y  y2 ) 


Figure  3.1  :  Voronoi  Diagram  of  Two  Points 


C.    VORONOI  DIAGRAM  OF  TWO  LINES 

In  the  case  that  a  world  consists  of  two  lines  Z^  and  L^  which  are  not  parallel 
to  each  other,  their  Voronoi  boundaries  consist  of  the  bisectors  of  two  lines  which 
generate  eight  Voronoi  regions  7(1^),  7(1^),  7(2^),  V(I»4)f  K(L„),  7(1^), 
V(Z,3)  and  V{L„)  (Figure  3.2). 
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Figure  3.2  :  Voronoi  Diagram  of  Two  Lines  (Not  Parallel) 

D.    VORONOI  DIAGRAM  OF  TWO  PARALLEL  LINES 

In  the  case  that  a  world  consists  of  two  parallel  lines  Ly  and  L^,  their  Voronoi 
boundary  is  one  line  which  is  parallel  to  L,  and  L^  which  generates  four  Voronoi 
regions  7(1^,  V(Z,a),  V(M  and  Vd^),  (Figure  3.3). 


A- 

v<M 

Voronoi  boundary 

V(L,2) 

w- 

v<M 

V{Ln) 

Figure  3.3  :  Voronoi  Diagram  of  Two  Parallel  Lines 


E.    VORONOI  DIAGRAM  OF  A  LINE  SEGMENT 


In  the  case  that  a  world  consists  of  one  closed  line  segment  pxp2 ,  we  treat  it 
as  a  union  of  three  objects  :  two  end  points  px,  p2  and  an  open  line  segment 


e  -  P1P2  •  A  closed  line  segment  includes  both  endpoints,  but  an  open  line  segment 
does  not.  Therefore,  its  Voronoi  boundaries  consists  of  two  lines  which  generate 
four  Voronoi  regions  V(px)y  V(p2),  V(ex)  and  V(e2).  (Figure  3.4) 


Figure  3.4  Voronoi  Diagram  of  A  Line  Segment 


F.    VORONOI  DIAGRAM  OF  A  POINT  AND  A  LINE 

In  the  case  that  a  world  consists  of  a  point  pf  and  a  line  q0,  its  Voronoi 

boundary  is  a  parabola  which  generates  three  Voronoi  regions  V(pf),  V(^),  and 
V(e2).  The  point  pf  is  the  focus  and  the  line  q0  is  the  directrix  of  the  parabola 

(Figure  3.5). 


\              v(pf)       / 

/ \     Pf  1 

► o. -J 

Voronoi  boundary 

i    ei       h 

e2 

9o 

|             V(<?2) 

Figure  3.5  :  Voronoi  Diagram  of  A  Point  and  A  Line 

G.   VORONOI  DIAGRAM  OF  A  POINT  AND  TWO  LINES 

In  the  case  that  a  world  consists  of  a  point  p  that  is  between  two  parallel 
lines  Lj  and  Z^,  their  Voronoi  boundaries  are  two  parabolas  (focus  p,  directrix  !> 
and  focus  p,  directrix  L^)  and  the  bisector  of  the  line  I>  and  Lj  which  generate 
five  Voronoi  regions  V(p)t  V{ex),  V(e2)y  V(e3)  and  V(e4)  (Figures  3.6  and  3.7). 
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Fig  3.6  :  Voronoi  Diagram  of  A  Center  Point  and  Two  Lines 
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Figure  3.7  :  Voronoi  Diagram  of  A  Point  and  Two  Lines 


H.   VORONOI  DIAGRAM  OF  LINE  AND  A  RAY 

Assume  a  ray  is  a  kind  of  line  which  has  only  one  end  point  p.  In  the  case 
that  a  world  consists  of  a  line  L,  and  a  ray  Z^,  which  is  orthogonal  to  the  line  Lp 


their  Voronoi  boundaries  are  a  line  segment  pxp2 ,  and  two  bisectors  of  the  line  L^ 
and  the  ray  L^,  and  a  parabola  (focus  p  and  directrix  Lj),  which  generates  five 
Voronoi  regions  V(p),  V{ex),  V(e2),  V(e2)  and  V(e4)  (Figure  3.8). 


bisector  of 
Li  and  l_2 
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bisector  of 
Li  and  l_2 


parabola 


Line  Segment 


Vie,) 


Figure  3.8  :  Voronoi  Diagram  of  Line  and  Ray 


I     VORONOI  DIAGRAM  OF  TWO  LINES  AND  A  LINE  SEGMENT 

In  the  case  that  a  world  consists  of  two  parallel  lines  Z,,  L^  and  a  closed  line 


segment  p{p2,  which  is  parallel  to  lines  L^  and  Z^,  their  Voronoi  boundaries  are 
bisectors  of  the  line  Lj  and  Z^,  the  bisector  of  the  line  L,  and  the  line  segment 


pxp2 1  the  bisector  of  the  line  L^  and  the  line  segment  pxp2 ,  two  closed  line 


segment  /?3/?4  and  psp6  and  parabolas  (Figures  3.9  and  3.10). 
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Figure  3.9  :  Voronoi  Diagram  of  Two  Lines  and  A  Center  Line  Segment 
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Figure  3.10  :  Voronoi  Diagram  of  Two  Lines  and  A  Line  Segment 
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J.    VORONOI  DIAGRAM  OF  A  NORMAL  POLYGON 

In  the  case  that  a  world  has  only  one  normal  polygon,  we  treat  it  as  a  union 
of  vertices  (points)  pi  and  open  edges  e{ .  There  are  Voronoi  boundaries  to  the 

polygon  shown  in  (Figure  3.11). 
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V(p2) 

Figure  3.11 :  Voronoi  Diagram  of  A  Normal  Polygon 

K.   VORONOI  DIAGRAM  OF  AN  INVERTED  POLYGON 

In  the  case  that  a  world  has  only  one  inverted  polygon,  we  also  treat  it  as  a 
union  of  vertices  (points)  p.  and  open  edges  e, .  There  are  Voronoi  boundaries  in 

its  interior  (Figure  3.12). 
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Figure  3.12  :  Voronoi  Diagram  of  An  Inverted  Polygon 
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IV.     CURVE  GENERATION 

A.  CONFIGURATION 

To  navigate  a  rigid  body  robot  vehicle,  the  vehicle's  state  can  be  described  by 
its  current  configuration, 

q  =  (p,B)  (4.1) 

where  p  is  the  vehicle's  current  coordinate  position  (x,y),  and  9  is  the  vehicle's 
tangent  orientation  at  that  point. 

Let  k  be  the  derivative  of  tangent  orientation  with  respect  to  the  length 
along  the  trajectory  s  which  is  called  curvature. 

m=^  (4.2) 

ds 
In  this  case,  k  is  not  included  in  the  configuration.  However,  k  can  be  included 
in  the  configuration  if  k  is  required  for  the  calculation. 

B.  NEXT  FUNCTION 

In  order  to  compute  the  sequence  of  configurations,  it  is  suffice  to  compute 
the  next  configuration  at  each  step  As.  Given  As  and    —  ,  we  can  estimate  the 

vehicle's  next  configuration  at  s+  As  as  follows. 

1.      Short  Circular  Segment 

The  short  path  segment  between  s  and  s  +  As  is  approximated  by  a  circular 
curve  segment  (Figure  4.1). 
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Figure  4.1 :  Short  Circular  Segment 


Assume  configurations  of  both  end  points  are  q0  and  qv 

9o=((*o>:U0o)  =  ((o,o),O) 

Let  us  assume  A0  *  0 .  The  radius  of  the  short  circular  segment  is 


(4.3) 
(4.4) 


r  = 


As_ 
Ad 


Where 


A6=ex-  60 


(4.5) 


(4.6) 
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The  configuration  of  the  end  point  qx  is 


Qx  = 


r*  \ 


s      61 


((     rsin(A0)     ^ 

r(l-cos(A0)) 
A0 


(f     (sin(A0)/A0)Ay 
((l-cos(A0))/A0)Ay 


Ad 


(ifA0*O)      (4.7) 


> 


Qx  = 


ffrW 


yj 


0, 


K  wi   J 


(foX\ 


,0  , 


(if  A0  =  O) 


Using  the  Taylor  expansion  forms  for  the  sin  and  cos  functions 


(4.8) 


sin(A0)  =  A0-(A0)73!+(A0)s/5!4-  =       (A0)2     (A0)4 

0     "  Ad  3!     +     5! 

_    cos(Ag)  _  1  -  (1  -  (A0)2/2!+  (A0)4/4!-  (A0)6/6!- ••) 
A0  A6 


(4.9) 


1      (A0)2     (A0)4 


+ 


x2!        4!  6! 

Therefore,  the  configuration  of  the  point  q1  is 


A0 


(4.10) 


0i  = 


/Yr  Y\ 


1*1 


Y     (1  -  (A0)2/3!  +  (A0)4/5!  -  •  •  -)As 

(l/2!-(A0)74!  +  (A0)76!----)A0A 
Ad 


In  general, 


A0«1 
Therefore  equation  (4.11)  can  be  approximated  as 


J 
) 


(4.11) 


(4.12) 


ft 


(l-(A0)2/3!)Ay 
(l/2!-(A0)2/4!)A0As 


i  / 


v 


A0 


/ 


(4.13) 
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2.      Global  Position  Calculation 

By  using  the  composition  function  o  [Ref.  1]  which  is  a  two  dimensional 
coordinate  transformation  from  the  local  coordinate  system  (x^y^  to  the  global 
coordinate  system  (xQ,y0),  we  can  calculate  the  global  position  of  the  vehicle  at 

s+As  as  follows. 


q(s  +  As)  =  q0o  qx  = 


rxQ  +  xx  cos  60  -  Vj  sin  60  N 
y0  +  xx  sin  60  +  yl  cos  60 

Where  q0  and  q1  are  given  Equation  (4.3)  and  Equation  (4.13). 


(4.14) 
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V.  SAFE  NAVIGATION  USING  A  STEERING  FUNCTION 

We  use  the  derivative  of  curvature  as  the  only  control  variable  for  the 
vehicle. 


^  =  f(P,e,K)  (5.1) 

as 
Therefore  the  vehicle's  motion  is  controlled  only  through  changing  its  curvature. 


Ak  = 


(—) 
<ds  ) 


As  (5.2) 


We  propose  the  steering  function  in  the  following  form 


die 

—  =  -(aAK  +  bAd  +  cAd)  (5.3) 
ds 

or 

dK 

—  +  aAK  +  bAd  +  cAd  =  0  (5.4) 
ds 

where  a,  b,  c  are  positive  constants  and  Air,  A0,  Ad  are  variables.  Each 

evaluation  of  Ak-,  AO,  Ad  differs  in  the  situation  of  the  obstacles  (in  the  case  that 

the  obstacles  are  one  point  and  one  line,  the  obstacles  are  two  lines,  and  the 

obstacles  are  two  points). 

A.   LINE  TRACKING 

Consider  a  special  case  of  the  line  tracking  (Figure  5.1).  Assume  we  want  to 
track  the  X-axis  of  the  global  coordinate  system,  then  we  can  define  three 
positive  constants  a,b,c. 
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q  =  ((x,y),6,K) 

Ad  =  y 

r 

X 

Figure  5.1 :  Line  Tracking 


On  the  X-axis, 


**  =  **  =  0 


Therefore 

AK=K-Kim  =  K 

un 

Ae  =  e-eim  =  e 

un 

Ad  =  y 
Let  us  now  consider  a  short  path  segment  (Figure  5.2). 


(5.5) 

(5.6) 
(5.7) 
(5.8) 


y  J 

x 

ft 

-►  s 

Ax 

PS-^__ 

\ 

A 

X 

Figure  5.2  :  Short  Path  Segment 
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The  vehicle's  tangent  orientation  6  is  specified  by 


Then 


Ay  is  defined  as 


tan  0  =  ^  (5.9) 

Ax 


6  =  tan"1  ^  =  tan-1  y'  (as  Ax  ->  0) 
Ax 

(v')3     (v')5 

=  y~    3    +    5    "'"  (5'10) 


Ay  =  V(Ax)2+(Av)2  (5.11) 


Therefore 


ir^^F-fM-^1'— »        <"» 


From  the  definition 

d  ... 


-(tan~1v,)=    <**       =  ^ 

dx  i+(y)    i+(yy 


-f(tan-1y)  =  ^-r  =  --f-r  (5.13) 


From  Equation  (5.12)  and  Equation  (5.13) 


d  y 

r=jg=A(tan-y)=^(tan  y)=jTW=     y  (5 14) 

|       7TT777  (1+(y)S)l 

The;    ore 


djf     r 


^=4-=(1,+(y)  t  =y"a-Ky)2r2-3y(y)2(i+(y)2r3  (5.15) 

<&     «£      y  i  +  (y  )2 
dx 
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Assume 


Then  from  Equation  (5.10) 


From  Equation  (5.14) 


From  Equation  (5.15) 


y2«l  (5.16) 

y'(y")2«y'"  (5.17) 

Ad  =  y'  (5.18) 

AK  =  y"  (5.19) 

die 

—  =  y'"  (5.20) 
ds 


Equation  (5.4)  becomes  an  ordinary  differential  equation: 

?"+a?'+b?+cy  =  0  (5.21) 

(Z)3  +  aD2  +  bD  +  c)y  =  0  (5.22) 

Since  this  is  a  third  order  linear  homogeneous  ordinary  differential  equation  with 
constant  coefficients,  it  must  have  at  least  one  real  root.  If  it  has  a  non-negative 
root,  it  does  not  have  a  converging  solution.  If  it  has  a  complex  conjugate  root, 
the  solution  oscillates  even  if  it  decays.  Since  we  want  non-oscillatory  decaying 
solutions,  Equation  (5.22)  must  have  three  negative  roots  of  D.  Also  if  we  want  a 
critical  damping  solution  then  Equation  (5.22)  must  be  specified  to  have  a  triple 
root,  -k  (where  k  >  0). 

Di  +  aD2+bD  +  c  =  (D  +  k)3=D3  +  3kD2+3k2D  +  k3        (5.23) 
Therefore  if  we  choose 

a  =  3k  (5.24) 

b  =  3k2  (5.25) 

c  =  k3  (5.26) 

Equation  (5.22)  becomes 
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(D  +  k)3y  =  0  (5.27) 

Thus,  under  this  condition,  there  is  only  one  degree  of  freedom  in  choosing  the 
parameter  k  instead  of  three  parameters  a,  b  and  c.  We  defme 


■*o  =7 


(5.28) 


This  size  constant  s0  controls  the  distance  for  which  the  vehicle  runs  before  it 

gets  on  track.  A  smaller  size  constant  makes  the  transition  distance  smaller.  Thus 
s0  controls  the  sharpness  of  the  trajectory.  From  Equations  (5.3),  (5.24),  (5.25), 

(5.26)  and  (5.28),  the  revised  steering  function  becomes 


dK 
ds 


'O 


( 


K  \so) 


Ak+3 


\2 

—    A0  + 


\so 


-    Ad 


(5.29) 


Ak\  Ad,  Ad  are  evaluated  depending  upon  the  environment.  Let  consider 
three  situations  for  vehicle  navigation:  the  objects  are  two  points,  the  objects  are 
two  lines,  the  objects  are  one  point  and  one  line. 

B.   TWO  POINTS 

When  we  navigate  the  vehicle  safely  to  make  the  same  clearance  from  two 
points,  its  trajectory  becomes  a  line  which  is  a  bisector  of  the  two  points.  It  is  a 
Voronoi  boundary  of  the  two  points.  Let  consider  the  case  of  the  world  consists 
of  two  points  pj  and  p2  (Figure  5.3). 


23 


Pi=(xl,yl) 


p2=(x2*y2) 


Figure  5.3  :  Safe  Navigation  of  Two  Points 

1.  Evaluation  of  Ak 

When  the  vehicle's  configuration  is  q  =  (p,0,  k), 

Ak=k  (5.30) 

Since  final  value  of  k  on  the  Voronoi  boundary  is  zero. 

2.  Evaluation  of  AG 

Let  ^(p^j)  denote  the  orientation  from  p  to  pl  and  y¥(pyp2)  denote 

the  orientation  from  p  to  p2.  Let  a  be  the  difference  between  the  orientation 

^¥(p,px)  and  the  vehicle's  orientation  6;  p  is  the  difference  between  the  vehicle's 
orientation  6  and  *F(/?,/?2)  (Figure  5.4). 


^(AA 

V  A  = 

=  UpVj) 

Qz 

-iP,e,K) 

^ 

0 

H'taft) 

*  Vi 

^JfeOfc) 

Figure  5.4  :  Evaluation  of  AG 
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a  =  V(p,Pl)-e  (5.31) 

P  =  d-V(p,p2)  (5.32) 

When  vehicle  is  on  the  Voronoi  boundary, 

a  =  p  (5.33) 

Let  the  desired  orientation  be  6d .  At  the  start  point 

ed  =  fiofwia/izd^^^^^^^^y^po^+ycp^)  (5.34) 

Where  normalize  1  is  a  function  which  normalizes  its  argument  into  a  range  of 


by  addition  of  ±nn  if  necessary  and  p0  is  the  middle  point  between  p1 


and  p2.  At  the  other  point, 

^IMfelp^);,f^)  -  e,my  e„  (5.35) 

Where  6      is  the  vehicle's  previous  desired  orientation  6d  at  the  point.  Then  the 

variable  Ad  is  evaluated  as 

A6  =  e-6d  (5.36) 

3.      Evaluation  of  Ad 

Let  d{  be  the  distance  between  p  and  px,  and  d2  be  the  distance  from  p 
to  p2. 


4  =  V(*-*i)2+(y-:yi)2  (5-37> 

^  =  V^-^)2  +  (v-v2)2  (5.38) 

The  signed  variable  Ad  is  evaluated  as  follows  (Figures  5.5  and  5.6).  Note  that 
Ad  can  be  signed  positive  or  negative,  or  equal  zero. 
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A  = 
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V(p,pi)     / 

/<k 

Qz 

=  (/>, 

6^  \      ^ 

P2 

=  (x3 

,y2) 

Figure  5.5  :  Case  xF(p,p1)-xF(p,p2)>0 


P2=(X2^2) 


¥(P, 


q  =  (p,e,K) 


^(P,A) 


Pi=Up3'i) 


Figure  5.6  :  Case  xF(p,p1)-M/(p,p2)<0 

Arf  =  d2-4    (if¥(p,A)-¥(p,p2)>0)  (5.39) 

M  =  dl-d2    (if^(p,A)-^(p,p2)<0)  (5.40) 

4.      Simulation  Results 

The  use  of  this  steering  function  for  vehicle  motion  control  is 
demonstrated  by  algorithmic  simulation  and  by  use  on  the  existing  robot 
Yamabico  11.  The  result  is  shown  in  Figures  5.7  and  5.8.  Figure  5.7  shows  the 
case  px  =(0,100),  p2- (0,-100),  the  initial  configuration  of  the  vehicle  is 
4  =  ((-300, 50),  0,0),  where  there  are  eight  cases  of  the  initial  0  :  0,  45,  90,  135, 
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180,  225,  270,  315  degrees.  Figure  5.8  shows  the  case  where  the  initial 
configuration  of  the  vehicle  is  q  =  ((-300,-50),  0,0) . 
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Figure  5.7  :  Simulation  Result  of  Two  Points,  q  =  ((-300, 50), 0,0) 
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Figure  5.8  :  Simulation  Result  of  Two  Points,  q  =  ((-300,  -50), 9,0) 


C.    TWO  LINES 

When  we  navigate  the  vehicle  safely  to  make  the  same  clearance  from  two 
lines,  its  trajectory  becomes  a  line  which  is  a  bisector  of  two  directed  lines.  So  it  is 
a  Voronoi  boundary  of  two  lines.  Assume  a  world  consists  of  two  directed  lines  q1 
and  q2,  there  are  two  cases:  they  are  parallel  or  not  parallel  (Figures  5.9  and  5.10). 
Interestingly,  calculations  for  Ax-,  A0  and  Ad  are  identical  in  each  case. 
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Figure  5.9  :  Parallel  Directed  Lines 


Figure  5.10  :  Not  Parallel  Directed  Lines 

1.  Evaluation  of  Ak 

When  the  vehicle's  configuration  is  q  =  (p>0, k), 

Ak  =  k  (5.41) 

Since  final  value  of  k  on  Voronoi  boundary  is  zero. 

2.  Evaluation  of  A6 

When  the  vehicle  is  on  the  Voronoi  boundary  its  orientation  is  the 
average  of  0,  and  02.  Let  this  desired  orientation  be  6d, 
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6d  =  normalizeli6^62  -  0,  ]  +  0, 


(5.42) 


Where  normalize  1  is  a  function  which  normalizes  its  argument  into  a  range  of 

n  n 


2'2 


by  addition  of  ±nn  if  necessary.  Then  the  variable  Ad  is  evaluated  as 


Ae  =  e-e, 


(5.43) 


3.      Evaluation  of  Ad 

Let  d,  be  the  signed  distance  from  p  to  qv  and  d2  be  the  signed 
distance  from  p  to  q2. 


dl  =  -(x  -  jc,  )sin  6X  +  ( v  -  Vj  )cos  0, 
dj  =  -(x  -  x2  )sin  02  +  (y  -  v2  )cos  02 
The  signed  variable  Ad  is  evaluated  as 


(5.44) 
(5.45) 


Ad  = 


_di+d1 
2 


(5.46) 


The  signed  distance  from  p  to  qx  is  the  distance  between  p  and  qvlf  p 
is  on  the  left  of  ^ ,  then  ^  >  0  and  if  p  is  on  the  right  of  qx ,  then  ^  <  0  (Figure 
5.1 1).  A  similar  argument  holds  for  d^ . 


p 

«>0,h 

^ 

i 

Qx 

P 

Figure  5.11 :  Signed  Distance  from  p  to  q, 
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4.      Simulation  Res  At 

The  result  of  algorithmic  simulation  can  be  found  in  Figures  5.12,  5.13, 
5.14,  and  5.15. 

Figures  5.12  and  5.13  show  the  case  where  the  lines  are  parallel.  Figure 
5.12  shows  the  case  qx  =((0,1 00), 0,0),  q2  =((0,-100),0,0)  and  the  initial 
configuration  of  the  vehicle  is  #  =  ((0,50),  6,0),  where  there  are  eight  cases  of  the 
initial  6  :  0,  45,  90,  135,  180,  225,  270,  315  degrees.  Figure  5.13  shows  the  case 
where  the  initial  configuration  of  the  vehicle  is  q  =  ((0,-50),  0,0) . 

Figures  5.14  and  5.15  show  the  case  where  the  lines  are  not  parallel. 
Figure  5.14  shows  the  case  qx  =((0,0), 90,0),  q2  =((0,0), 0,0)  and  the  initial 
configuration  of  the  vehicle  is  q  =  ((50, 150), 6,0).  Figure  5.15  shows  the  case 
where  the  initial  configuration  of  the  vehicle  is  q  =  ((150,50),  6,0). 
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Figure  5.12  :  Simulation  Result  of  Parallel  lines,  q  =  ((0,50),8,0) 
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Figure  5.13  :  Simulation  Result  of  Parallel  Lines,  q  =  ((0,-50),9,0) 
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Figure  5.14  :  Simulation  Result  of  Not  Parallel  Lines,  q  =  ((50, 150), 6,0) 
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Figure  5.15  :  Simulation  Result  of  Not  Parallel  Lines,  q  =  ((150,50), 6,0) 

D.    ONE  POINT  AND  ONE  LINE 

When  we  navigate  the  vehicle  safely  to  make  the  same  clearance  from  one 
point  and  one  line,  its  trajectory  becomes  a  parabola.  So  it  is  a  Voronoi  boundary 
of  a  point  and  a  line. 

1.      Definition  of  Parabola 

When  a  world  consists  of  a  point  pf  and  a  directed  line  q0 ,  its  Voronoi 

boundary  becomes  a  parabola.  A  parabola  is  defined  as  the  focus  pf  and  the 
directrix  q0  : 

Pf=(xf,yf)  (5.47) 

<7o=OWoA)  (5-48> 

The  directrix  q0  has  a  direction,  and  hence,  parabola  has  a  direction.  Let 
1  be  the  signed  distance  from  pf  to  qQ . 
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1=  -(x,  - x0)sin60  +  (yf  - y0)cosd0  (5.49) 

The  signed  distance  from  pf  to  q0  is  the  distance  between  pf  and  q0 .  If  pf  is  on 
the  left  of  q0 ,  then  1>  0  and  if  pfis  on  the  right  of  q0 ,  then  1<  0  (Figure  5.16). 


Figure  5.16  :  Signed  Distance  from  pf  to  qa 

a.    Case  1>  0 

Let  6y  denote  the  orientation  of  the  normal  ray  from  pf  to  q0 .  We 

define  a  polar  coordinate  system  whose  pole  is  pf  and  whose  initial  ray  is  0l 
(Figure  5.17). 


q0=(x0,y0,60) 


V(pf>p) 


P  =  (r,0) 


ex  =  e0-ni2 


Figure  5.17  :  Parabola  (1  >  0) 
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0,=0o-?r/2  (5.50) 

In  this  system,  p  is  represented  by  (r,0),  where  r  is  the  distance  between  pf  and  p 
and  0  is  the  orientation  from  pf  to  /?  taken  from  the  initial  ray.  In  this  case,  we 
take  0  (-n  <  0  <  n)  counterclockwise  from  the  initial  ray.  The  coordinate  of  p  in 

the  global  Cartesian  system  is 

(x,y)  =  (*/  +  rcos(6{  +  <p),yf  +  rsin(6l  +  0)) 

(5.51) 


lsin(fl0  +  0)  lcos(fl0  +  0) 

xf+     i_l^ca     ,yf 


1  +  COS0  1  +  COS0 

Let  Vip^p)  denote  the  orientation  from  pf  to  p.  By  definition,  the  angle  a 
between  ¥(/?,,/>)  and  the  orientation  *F  of  the  tangent  atp  is  defined  as 

a«£-±  (5.52) 

2      2 

The  orientation  *F  of  the  tangent  at  p  in  the  polar  coordinate  system  is 

'-♦—♦♦(MM*!         (5-53) 

The  orientation  0  of  this  tangent  at  p  in  the  global  coordinate  system  is 

•-«+*-(*-!MMM+*     (5-54) 

Let  s  denote  the  arc  length.  Then  the  curvature  k  aip  is 


35 


de  i 

:c_dd_d<t>_        2 

ds      ds 


d<f>     *\j  dtp 

(1  +  COS0)2 


/  dr .  2       2 
(-77)  +  r 


y(l  +  COS0)2        (1  +  COS0)4 
(1  +  COS0)2  1 


2lV(l  +  cos0)2+sin20      2lV2  +  2cos0      2V21 


(1  +  COS0) 


2  cos2  - 
v         v2>/; 


2      1       3f0 
=  -cos    — 

1       U 


(5.55) 


2V21 
b.     Case  1<0 

In  this  case  (Figure  5.18),  the  orientation  8l  of  the  initial  ray  in  the 

global  coordinate  system  is 

^  =  00  +  ^/2  (5.56) 


9Q=(xQiy0l60) 


e,  =  en  +  n/2 


i 


iL         P  =  (r,f) 


^(P/,P) 


Figure  5.18  :  Parabola  (1<  0) 

We  take  <J>  (-n  <  <p  <  n)  clockwise  from  the  initial  ray. 

*  =  -*'  (5.57) 

Then  the  coordinate  of  p  in  the  global  Cartesian  system  is  defined  by  Equation 
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(5.51),  the  orientation  6  of  the  tangent  at  p  in  the  global  coordinate  system  is 
defined  by  Equation  (5.55)  and  the  curvature  k  atp  is  defined  by  Equation 
(5.56). 

2.      Evaluation  of  Ak 

When  the  vehicle's  configuration  is  q  -  (p,  0,  k)  ,  assume  the  intersection 
of  the  orientation  W(p,pf)  and  the  parabola  is  q^  =  {p^B^K^)  as  shown  in 

Figure  5.19.  The  variable  A*r  is  evaluated  as 


Ak= k- k 


para 


(5.58) 


Note  that  Ak  converges  to  zero  as  q  approaches  to  q 


para  ' 


q  =  (p,6yK) 


0,  =  0o-*/2 


Figure  5.19  :  Evaluation  of  Ak 
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3.      Evaluation  of  A6 

Let  a  be  the  difference  between  the  vehicle's  orientation  6  and  the 
orientation  0l  of  the  initial  ray.  Also  let  P  be  the  difference  between  the 
orientation  *¥{p,pf)  and  the  vehicle's  orientation  6  (Figure  5.20). 


q  =  (p,eyK) 


Figure  5.20  :  Evaluation  of  A6 


Then 


When  vehicle  is  on  the  parabola, 


P=e-e1 

a  =  p 


Let  this  desired  orientation  be  0d> 


(5.59) 
(5.60) 

(5.61) 
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6d  =  normalizel 


C¥{p,pf)-ex 


-en 


+  en 


(5.62) 


Where  normalizel  is  a  function  which  normalizes  its  argument  into  a  range  of 

71    It 


2'2 


by  addition  of  ±nn  if  necessary.  Then  the  variable  A0  is  evaluated  as 


Ad  =  e-6, 


(5.63) 


4.     Evaluation  of  Ad 

Let  d1  be  the  distance  between  p  and  pf,  and  d2  be  the  signed  distance 

from  p  to  qQ  (Figure  5.21). 

dy  =  ^(x-xf)2  +  (y-yf)2  (5.64) 

cLt  = -(x  -  x0)  sin  60  +  (y-y0)  cos  60  (5.65) 


Figure  5.21 :  Evaluation  of  Ad 


The  signed  variable  Ad  is  evaluated  as 
Ad^d.-d,  (if  1>0) 
Ad  =  d2+d!(if  KO) 


(5.66) 
(5.67) 
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5.      Simulation  Result 

The  result  of  algorithmic  simulation  can  be  found  in  Figures  5.22,  5.23, 

5.24  and  5.25.  Figures  5.22  and  5.23  show  the  case  where  the  1  is  positive. 

Figures  5.24  and  5.25  show  the  case  where  the  1  is  negative. 

Figure  5.22  shows  the  case  of  pf  =(0,200),  qQ  =((0,0), 0,0),  the  initial 

configuration  of  the  vehicle  is  q  =  ((-300,200),  8,0),  where  there  are  eight  cases  of 
the  initial  6  :  0,  45,  90,  135,  180,  225,  270,  315  degrees.  Figure  5.23  shows  the 
case  where  the  initial  configuration  of  the  vehicle  is  q  =  ((-200,300),  0,0) . 

Figure  5.24  shows  the  case  of  pf  =(0,-200),  q0  =((0,0), 0,0),  the  initial 

configuration  of  the  vehicle  is  #  =  ((-300,  -200),  6,0),  where  there  are  eight  cases 
of  the  initial  6  :  0,  45,  90,  135,  180,  225,  270,  315  degrees.  Figure  5.25  shows  the 
case  where  the  initial  configuration  of  the  vehicle  is  q  =  ((-200,-300), 6,0) . 
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Figure  5.22  :  Simulation  Result  of  1>  0,  q  =  ((-300, 200), 9,0) 
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Figure  5.23  :  Simulation  Result  of  1>  0,  q  =  ((-200, 300), 0,0) 
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Figure  5.24  :  Simulation  Result  of  1<  0,  q  =  ((-300, -200), 6,0) 
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Figure  5.25  :  Simulation  Result  of  1<  0,  q  =  ((-200, -300), 0,0) 
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VI.  CONCLUSION 

A.   RESULTS 

Simulation  results  shows  that  when  the  objects  are  two  points,  two  lines,  or 
one  point  and  one  line,  we  can  safely  navigate  the  vehicle  to  achieve  equal 
clearances  from  these  objects.  The  motion  of  the  vehicle  is  optimized  at  each 
point  directly  from  the  information  of  the  obstacles  near  the  vehicle.  After 
calculating  the  steering  function  dtc/ds  which  is  the  derivative  of  the  curvature  of 
the  vehicle  motion,  the  vehicle  follows  the  Voronoi  boundaries  defined  by  the 
environment. 

Previous  work  in  the  motion  control  of  the  autonomous  vehicle  Yamabico  1 1 
used  path  tracking  using  a  path  specification  for  lines,  circles  and  parabolas 
which  are  images  of  the  path.  Before  the  mission  began  the  vehicle  was  given  a 
track  to  follow;  motion  planning  consisted  of  calculating  the  point  on  the  track 
closest  to  the  vehicle  and  then  steering  the  vehicle  to  get  onto  the  track. 

If  the  world  navigated  by  the  robot  is  large,  it  is  complicated  and  inefficient  to 
calculate  every  Voronoi  boundary  of  this  world.  It  is  better  to  calculate  the 
optimal  motion  of  the  vehicle  at  each  point  directly  from  the  information  of  the 
world  by  computing  the  steering  function  dK/ds  without  calculating  the  track  to 
follow. 

Unlike  path  tracking  method,  this  method  can  be  applied  to  avoid  moving 
objects  since  we  calculate  the  optimal  motion  of  the  vehicle  at  each  point  directly 
from  the  information  of  the  world.  Additionally,  motion  control  is  simpler  and 
faster  than  in  the  path  tracking  method. 
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B.    RECOMMENDATIONS 

Based  on  the  results  of  this  thesis,  recommended  follow  on  work  includes 
leaving  point  calculation. 

There  are  two  goals  for  planning  autonomous  vehicle  navigation  planning: 
shortest  path  and  safe  path.  This  safe  navigation  method  is  for  only  safe  path 
planning.  The  short  path  planning  is  represented  by  path  tracking  using  a  path 
specification  for  lines,  circles  and  parabolas  which  are  images  of  the  path.  When 
we  will  combine  this  safe  navigation  method  with  short  path  planning,  it  will  be 
necessary  to  calculate  the  leaving  point  from  one  path  to  another.  Afterwards,  the 
vehicle  will  continue  on  its  way  smoothly. 
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APPENDIX 

This  appendix  contains  the  C  code  for  safe  navigation  which  generated  the 
results  found  in  this  thesis. 

A.   POINTPATH.C 

Author:  Masahide  Shirasaka 
Project:     'amabico  Robot  Control  System 
Date:  me  25  1994 

Revised  July  12  1994 

File  Nan  pointpath.C 

Environment:     GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:       This  Program  contains  functions  for  safe  navigation 
when  the  obstacles  are  two  points. 

#include  <stdio.h> 

#include  <math.h> 

#defmeDR  (PI/180.0) 

#define  PI      3.14159265358979323846 

//  =PI 
#define  DPI     6.28318530717958647692 

//  =  2.0*PI 
#define  HPI     1.57079632679489661923 

//  =  PI/2.0 
#define  RAD     57.29577951308232087684 

//  =180.0/PI 

HLE*fp0,*fpl; 

struct:  POINT 

typedef  struct  { 
double  x; 
double  y; 

} 
POINT; 

struct:  CONFIGURATION 
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typedef  struct  { 
POINT  point; 
double  theta; 
double  kappa; 

} 
CONFIGURATION; 

Function:       normalize() 

Purpose:         This  procedure  is  for  a  function  which  normalizes  an  angle 
to  within  +  or  -  PI  values. 

double  normalize(double  angle) 

{ 

angle  =  angle  -  DPI*(ceil((angle  +  PI)/DPI)  - 1.0); 

return(angle); 

} 

Function:       normalizel() 

Purpose:        This  procedure  is  for  a  function  which  normalizes  an  angle 
to  within  +  or  -  PI/2.0  values. 

double  normalize  1  (double  angle) 

{ 
while(angle  >  PI/2.0) 

{ 

angle  =  angle  -  PI; 

} 

while(angle  <=  -PI/2.0) 

{ 

angle  =  angle  +  PI; 

} 
return(angle); 

} 

FUNCTION:  InputPoints() 

Purpose:        This  procedure  Inputs  the  configurations  of  two  points. 

void  InputPoints(POINT  &pl  ,POINT  &p2) 

{ 
/*  Point  obstacle  pi  */ 
printf("Input  Coordinates  of  the  pi .  \n"); 
printf("X=V); 
scanf("%lf\&pl.x); 
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printf("Y=\n"); 
scanf("%lf',&pl.y); 

/*  Point  obstacle  p2  */ 
printf("Input  Coordinates  of  the  p2.  \n"); 
printf("X=W); 
scanf("%lf',&p2.x); 
printf("Y=W); 
scanf("%lf',&p2.y); 
} 

FUNCTION:  InputlnitConfigO 

Purpose:         This  procedure  Inputs  the  initial  Configration  of  the  vehicle, 
size  constant  and  step  size. 

void  InputInitConfig(CONHGURATION  &q,double  &sO,double  &DS) 

{ 
/*  Config  of  q  */ 

printf("Input  initial  Configuration  of  the  vehicle  q.  \n"); 
printf("X=\n"); 
scanf("%lf',&q.pointx); 
printf("Y=W); 
scanf("%lf',&q.poinLy); 
printf("theta=  \n"); 
scanf("%lf',&q.theta); 
q.theta=normalize(q.theta/RAD); 
printf("kappa=W); 
scanf("%lf',&q.kappa); 

/*  Size  constant  */ 

printf("Input  the  size  constant  sO\n"); 

printf("sO=W); 

scanf(M%lf',&sO); 

/*  DS  */ 

printfflnput  the  step  size  DS.Nn"); 

printfCDS  =Nnn); 

scanf("%lf,&DS); 

} 

FUNCTION:  GetInitThetaDesire() 

Purpose:        This  procedure  is  for  a  function  which  compute  the  value  of 
desired  initial  theta. 

double  GetInitThetaDesire(CONHGURATION  qJ>OINT  pl,POINT  p2) 
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{ 

POINT  pO; 

p0.x  =  (pl.x  +  p2.x)/2.0; 
p0.y  =  (pl.y  +  p2.y)/2.0; 

return(normalizel((atan2(pl  .y-q.point.y,pl  .x-q.point.x) 
+  atan2(p2.y-q.point.y,p2.x-q.point.x))/2.0 

-  atan2(p0.y-q.point.y,p0.x-q.point.x) ) 
+  atan2(p0.y-q.point.y,p0.x-q.point.x)); 

} 

FUNCTION:  GetConstants() 

Purpose:        This  procedure  is  for  a  function  which  compute  the  value  of 
constants  k,  a,  b,  and  c. 

void  GetConstants(double  S0,double  &a,double  &b,double  &c) 

{ 
double  ConstK; 

ConstK=1.0/S0; 
a=3.0*ConstK; 
b=3.0*ConstK*ConstK; 
c=ConstK*ConstK*ConstK; 

} 

I*  ********************************************************* 

FUNCTION:  GetSteeringFunc() 

Purpose:        This  procedure  is  for  a  function  which  compute  the  value  of 

steering  function  dk/ds. 

**********************************************************/ 

void  GetSteering(double  a^double  b,double  c,CONFIGURATION  q, 

POINT  pl^OINT  p2,double  &thetaDesire,double  &u) 

{ 
double  deltaKappa,deltaTheta,deltaDist,dl  ,d2; 

/*  Calculate  deltaKappa  */ 
deltaKappa  =  q.kappa; 

/*  Calculate  deltaTheta  */ 

thetaDesire  =  normalizel((atan2(pl.y-q.point.y,pl.x-q.point.x) 
+  atan2(p2.y-q.point.y,p2.x-q.point.x))/2.0 

-  thetaDesire)  +  thetaDesire; 
deltaTheta  =  normalize(q.theta  -  thetaDesire); 

/*  Calculate  deltaDist  */ 

dl  =  sqrt((pl.x-q.point.x)*(pl.x-q.pointx) 
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+  (pi  .y-q.point.y)*(pl  .y-q.point.y)); 
62  =  sqrt((p2.x-q.point.x)*(p2.x-q.point.x) 

+  (p2.y-q.point.y)*(p2.y-q.point.y)); 
if  (atan2(pl  .y-q.point.y,pl  .x-q.pointx) 

-  atan2(p2.y-q.point.y,p2.x-q.pointx)>0) 
deltaDist  =  d2-dl; 
else 

deltaDist  =  dl  -  d2; 

/*  Calculate  Steering  fucnction  =  u  */ 

u  =  -(a*deltaKappa  +  b*deltaTheta  +  c*deltaDist); 

} 

!********+.  ************************************************* 

FUNCTION:  GetDkappa() 

Purpose:         This  procedure  is  for  a  function  which  computes  the  value  of 

dKappa. 

**********************************************************/ 

double  GetDkappa(double  u,double  ds) 

{ 
return(u*ds); 

} 

/********************************************************** 

FUNCTION:  GetKappa() 

Purpose:         This  procedure  is  for  a  function  which  computes  the  value  of 

Kappa. 

**********************************************************/ 

void  GetKappa(double  dkappa,CONFIGURATION  &q) 

{ 
q.kappa=q. kappa  +  dkappa; 

} 

i********************************************************** 

FUNCTION:  GetDtheta() 

Purpose:         This  procedure  is  for  a  function  which  computes  the  value  of 
dtheta. 

double  GetDtheta(CONFIGURATION  ql, double  ds 

{ 
return(ql. kappa  *  ds); 

} 

FUNCTION:  next() 

Purpose:        This  procedure  is  for  a  function  which  computes  the  next 
configration  of  the  vehicle. 
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void  next(double  ds,double  dtheta,double  &s,CONFIGURATION  &q) 

{ 

CONFIGURATION  ql; 

/*  CONFIGURATION  of  ql  */ 

ql.pointx  =  (1.0  -  dtheta*dtheta/6.0)*ds; 

ql.point.y  =  (0.5  -  dtheta*dtheta/24.0)*dtheta*ds; 

ql.theta  =  dtheta; 

s  =  s  +  ds* 

/*  CONFIGURATION  of  q  */ 

q.pointx  =  q.point.x  +  ql.point.x*cos(q.theta)  -  ql.point.y*sin(q.theta); 
q.point.y  =  q.point.y  +  ql.pointx  *sin(q.theta)  +  ql.pointy*cos(q.theta); 
q.theta  =  q.theta  +  ql.theta; 
} 

FUNCTION:  Openfile() 

Purpose:        This  procedure  opens  the  output  file. 

void  Openfile(CONHGURATION  q,double  s) 

{ 

fpO  =  fopen("path.dat","w"); 

fpl  =  fopen("path","w"); 

fprintf(fpO,"  s  x  y     theta[deg]  kappa  "); 

fprintf(fpO,"   u   deltaKappa  deltaTheta  deltaDistW); 

printf("  s        x        y       theta[deg]  kappaW); 

fprintf(fp0,"%4.4f  %4.4f  %4.4f  %4.4f  %4.4f\n",s,  q.pointx,  q.pointy, 

q.theta*RAD,q.kappa); 

printf("%4.4f  %4.4f  %4.4f  %4.4f  %4.4f\n",  s,  q.pointx,  q.pointy, 

q.theta*RAD,q.kappa); 

fprintf(fpl,"%f  %f\n",  q.pointx,  q.pointy); 
} 

FUNCTION:  Printfile() 

Purpose:        This  procedure  prints  the  result  to  the  file. 

void  Printfile(CONHGURATION  q,double  s) 

{ 

fprintf(fp0,"%4.4f  %4.4f  %4.4f  %4.4f  %4.4f   ", 

s,  q.pointx,  q.pointy,  q.theta*RAD,q.kappa); 
printf("%4.4f  %4.4f  %4.4f  %4.4f  %4.4fsn", 

s,  q.pointx,  q.pointy,  q.theta*RAD,q.kappa); 
fprintf(fpl,"%f  %fvn",  q.pointx,  q.pointy);    /*  for  gnuplot  */ 
} 
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/****** **************************************************** 

INUNCTION:  mainO 

*****************»****************************************/ 

void  main(void) 

{ 

CONFIGURATION  q; 
POINT  pi, p2; 

double  u;  /*  steering  function  */ 
double  DS,s,sO,a,b,c,thetaDesire; 
double  dkappa,dtheta; 

InputPoints(pl  ,p2); 

InputInitConfig(q,sO,DS); 

GetConstants(sO,a,b,c); 

thetaDesire=GetInitThetaDesire(q,pl  ,p2); 

s  =  0.0; 
Openfile(q,s); 

do 

{ 
GetSteering(a,b,c,q,pl  ,p2,thetaDesire,u); 
dkappa  =  GetDkappa(u,DS); 
GetKappa(dkappa,q); 
dtheta=GetDtheta(q,DS); 
next(DS,dtheta,s,q); 
Printfile(q,s); 

} 
while(s<=800.0); 

fclose(fpO); 
fclose(fpl); 

} 


B.    LINEPATH.C 

/********************************************************** 

Author:    Masahide  Shirasaka 

Project:    Yamabico  Robot  Control  System 

Date:       June  26  1994 

Revised:  July  12  1994 

File  Name:  linepath.C 

Environment:     GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
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Description:        This  Program  contains  functions  for  safe  navigation 
when  the  obstacles  are  two  directed  lines. 

#include  <stdio.h> 

#include  <math.h> 

#defineDR  (PI/180.0) 

#defme  PI     3.14159265358979323846 

//  =PI 
#define  DPI     6.28318530717958647692 

//   =2.0*PI 
#define  HPI     1.57079632679489661923 

//  =  PI/2.0 
#define  RAD     57.29577951308232087684 

//  =180.0/PI 

HLE*fpO,*fpl; 

struct:  POINT 

typedef  struct  { 
double  x; 
double  y; 

} 
POINT; 

struct:  CONFIGURATION 

typedef  struct  { 
POINT  point; 
double  theta; 
double  kappa; 

} 
CONFIGURATION; 

Function:       normalize(angle) 

Purpose:        This  procedure  is  for  a  function  which  normalizes  an  angle 
to  within  +  or  -  PI  values. 

double  normalize(double  angle) 

{ 

angle  =  angle  -  DPI*(ceil((angle  +  PI)/DPI)  - 1.0); 

return(angle); 
} 
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Function:       normalize  1  (angle) 

Purpose:         This  procedure  is  for  a  function  which  normalizes  an  angle 
to  within  +  or  -  PI/2.0  values. 

double  normalize  1  (double  angle) 

{ 
while(angle  >  PI/2.0) 

{ 

angle  =  angle  -  PI; 

} 

while(angle  <=  -PI/2.0) 

{ 

angle  =  angle  +  PI; 

} 
return(angle); 

} 

FUNCTION:  InputLines() 

Purpose:        This  procedure  Inputs  the  configurations  of  two  Lines. 

void  InputLines(CONHGURATION  &ql, CONFIGURATION  &q2) 

{ 
/*  Line  obstacle  ql  */ 

printf("Input  initial  Configuration  of  ql .  W); 
printf("X=\n"); 
scanf("%lf',&ql  .pointx); 
printf("Y=W); 
scanf("%lf  *,&ql  .pointy); 
printf("theta=W); 
scanf("%lf ',&ql  .theta); 
ql  .theta=normalize(ql  .theta/RAD); 
ql.kappa=0.0; 

/*  Line  obstacle  q2  */ 

printf("Input  initial  Configuration  of  q2.  \n"); 
printf("X=W); 
scanf("%lf',&q2.point.x); 
piintf("Y=W); 
scanf("%lf',&q2.point.y); 
printf("theta=\n"); 
scanf("%lf\&q2.theta); 
q2.theta=normalize(q2.theta/RAD); 
q2.kappa=0.0; 
} 
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FUNCTION:  InputlnitConfigO 

Purpose:         This  procedure  Inputs  the  initial  Configration  of  the  vehicle, 
size  constant  and  step  size. 

void  InputInitConfig(CONFIGURATION  &q,double  &sO,double  &DS) 

{ 
/*  Config  of  q  */ 

printf("Input  initial  Configuration  of  the  vehicle  q.  Nn"); 
printf("X=Nn"); 
scanf("%lf\&q.poinLx); 
printf("Y=\n"); 
scanf("%lf\&q.pointy); 
printf("theta=  \n"); 
scanf("%lf',&q.theta); 
q.theta=normalize(q.theta/RAD); 
printf("kappa=W); 
scanf("%lf*,&q.kappa); 

/*  Size  constant  */ 

printf("Input  the  size  constant  sCNn"); 

printf("sO=V); 

scanf("%lf\&sO); 

/*  DS  */ 

printf ("Input  the  step  size  DS.W); 

printf("DS=V); 

scanf("%ir,&DS); 

} 

FUNCTION:  GetConstants() 

Purpose:         This  procedure  is  for  a  function  which  computes  the  value  of 
constants  k,  a,  b,  and  c. 

void  GetConstants(double  S0,double  &a,double  &b,double  &c) 

{ 
double  ConstK; 

ConstK=1.0/S0; 
a=3.0*ConstK; 
b=3.0*ConstK*ConstK; 
c=ConstK*ConstK*ConstK; 

} 

FUNCTION:  GetSteeringFunc() 
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Purpose:         This  procedure  is  for  a  function  which  computes  the  value  of 
steering  function  dk/ds. 

double  GetSteering(double  a,double  b,double  c, CONFIGURATION  q, 
CONFIGURATION  ql, CONFIGURATION  q2) 

{ 
double  deltaKappa,deltaTheta,deltaDist,thetaDesire,dl  ?d2; 

/*  Calculate  DeltaKappa  */ 
deltaKappa  =  q.kappa; 

/*  Calculate  DeltaTheta  */ 

thetaDesire  =  normalizel((ql.theta+q2.theta)/2.0  -  ql.theta)  +  ql.theta; 

deltaTheta  =  normalize(q.theta  -  thetaDesire); 

/*  Calculate  DeltaDist  */ 

dl  =  -(q.pointx  -  ql.point.x)*sin(ql.theta) 

+  (q.pointy  -  ql.point.y)*cos(ql.theta); 
d2  =  -(q.pointx  -  q2.point.x)*sin(q2.theta) 

+  (q.point.y  -  q2.point.y)*cos(q2.theta); 
deltaDist=(dl+d2)/2.0; 

/*  Calculate  Steering  fucnction  =  u  */ 
retum(-(a*deltaKappa  +  b*deltaTheta  +  c*deltaDist)); 

} 

FUNCTION:  GetDkappa() 

Purpose:         This  procedure  is  for  a  function  which  computes  the  value  of 
dKappa. 

double  GetDkappa(double  u,double  ds) 

{ 
return(u*ds); 

} 

FUNCTION:  GetKappa() 

Purpose:        This  procedure  is  for  a  function  which  computes  the  value  of 
Kappa. 

void  GetKappa(double  dkappa,CONHGURATION  &q) 

{ 
q.kappa=q. kappa  +  dkappa; 

} 


/ 
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FUNCTION:  GetDtheta() 

Purpose:         This  procedure  is  for  a  function  which  computes  the  value  of 
dtheta. 

double  GetDtheta(CONFIGURATION  ql, double  ds) 

{ 
return(ql. kappa  *  ds); 

} 

FUNCTION:  next() 

Purpose:        This  procedure  is  for  a  function  which  computes  the  next 
configration  of  the  vehicle. 

void  next(double  ds,double  dtheta,double  &s,CONFIGURATION  &q) 

{ 

CONFIGURATION  ql; 

/*  CONHGURATION  of  ql  */ 
ql.point.x  =  (1.0  -  dtheta*dtheta/6.0)*ds; 
ql.point.y  =  (0.5  -  dtheta*dtheta/24.0)*dtheta*ds; 
ql  .theta  =  dtheta; 

s  =  s  +  ds; 

/*  CONHGURATION  of  q  */ 

q.pointx  =  q.pointx  +  ql  .pointx*cos(q.theta)  -  ql.point.y*sin(q.theta); 
q.point.y  =  q.point.y  +  ql.pointx*sin(q.theta)  +  ql.pointy*cos(q.theta); 
q.theta  =  q.theta  +  ql  .theta; 
} 

FUNCTION:  Openfile() 

Purpose:        This  procedure  opens  the  output  file. 

void  Openflle(CONHGURATION  q,double  s) 

{ 
fpO  =  fopen("path.dat","w"); 
fpl  =  fopen("path","w"); 

fprintf(fpO,"  s  x  y     theta[deg]  kappa  "); 

fprintf(fpO,"   u   deltaKappa  deltaTheta  deltaDistNn"); 
printf("  s        x        y       theta[deg]  kappaNn"); 

fprintf(fp0,"%4.4f  %4.4f  %4.4f  %4.4f  %4.4f\n",s,  q.pointx,  q.point.y, 

q.theta*RAD,q.kappa); 
printf("%4.4f  %4.4f  %4.4f  %4.4f  %4.4Ni",  s,  q.pointx,  q.pointy, 

q.theta*RAD,q.kappa); 
fprintf(fpl,"%f  %f\n",  q.pointx,  q.pointy); 
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} 

FUNCTION:  PrintfileO 

Puipose:         This  procedure  prints  the  result  to  the  file. 

******************************************* *********±*****i 

void  Printfile(CONFIGURATION  q,double  s) 

{ 
fprintf(fp0,"%4.4f  %4.4f  %4.4f  %4.4f  %4.4f   ", 

s,  q.point.x,  q.pointy,  q.theta*RAD,q.kappa); 
printf("%4.4f  %4.4f  %4.4f  %4.4f  %4.4f\n", 

s,  q.pointx,  q.point.y,  q.theta*RAD,q.kappa); 
fprintf(fpl,"%f  %f\n",  q.pointx,  q.pointy);    /*  for  gnuplot  */ 
} 

i*************** ******************************************* 

FUNCTION:  mainO 

void  main(void) 

{ 
CONFIGURATION  q,ql,q2; 
double  u;  /*  steering  function  */ 
double  DS,s,sO,a,b,c; 
double  dkappa,dtheta; 

InputLines(ql  ,q2); 

InputInitConfig(q,sO,DS); 
GetConstants(sO,a,b,c); 

s  =  0.0; 
Openfile(q,s); 

do 

{ 
u  =  GetSteering(a,b,c,q,ql,q2); 
dkappa  =  GetDkappa(u,DS); 
GetKappa(dkappa,q); 
dtheta=GetDtheta(q,DS); 
next(DS,dtheta,s,q); 
Printfile(q,s); 

} 
while(s<=800.0); 

fclose(fpO); 
fclose(fpl); 

} 
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C.    PARAPATH.C 

Author:    Masahide  Shirasaka 

Project:    Yamabico  Robot  Control  System 

Date:        May  15  1994 

Revised:  June  17  1994 

File  Name:  parapath.C 

Environment:     GCC  ANSI  C  compiler  for  the  motorola  68020  processor 

Description:       This  Program  contains  functions  for  safe  navigation 

when  the  obstacles  are  one  point  and  one  directed  line. 

#include  <stdio.h> 

#include  <math.h> 

#defineDR  (PI/180.0) 

#define  PI      3.14159265358979323846 

//  =PI 
#define  DPI    6.28318530717958647692 

//  =2.0*PI 
#define  HPI     1.57079632679489661923 

//  =  PI/2.0 
#define  RAD     57.29577951308232087684 

//  =180.0/PI 

FILE  *fjp0,  *fpl,  *fp2,  *fp3; 


struct:  POINT 

♦  ate********************************************************/ 

typedef  struct  { 
double  x; 
double  y; 

} 
POINT; 

struct:  CONFIGURATION 

typedef  struct  { 
POINT  point; 
double  theta; 
double  kappa; 

} 
CONFIGURATION; 
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Function:       normalize() 

Purpose:         This  procedure  is  for  a  function  which  normalizes  an  angle 
to  within  +  or  -  PI  values. 

double  normalize(double  angle) 

{ 
angle  =  angle  -  DPI*(ceil((angle  +  PI)/DPI)  -  1.0); 
return(  angle); 

} 

Function:       normalizel() 

Purpose:         This  procedure  is  for  a  function  which  normalizes  an  angle 
to  within  +  or  -  PI/2.0  values. 

*************************************************** 'jc  **■:+:#**./ 

double  normalize  1  (double  angle) 

{ 
while(angle  >  PI/2.0) 

{ 

angle  =  angle  -  PI; 

} 

while(angle  <=  -PI/2.0) 

{ 

angle  =  angle  +  PI; 

} 
return(angle); 

} 

FUNCTION:  InputParabola() 

Purpose:         This  procedure  Inputs  the  Configrations  of  one  point  and 
one  directed  line. 

void  InputParabola(CONFIGURATION  &q0 ,POINT  &p) 

{ 
/*ConfigofqO*/ 

printf("Input  initial  Configuration  of  the  qO  (directrix).  W); 
printf("X=\n"); 
scanf("%lf",&q0.point.x); 
printf("Y=\n"); 
scanf("%lf',&q0.point.y); 
printf("theta=  \n"); 
scanf("%lf\&q0.theta); 
q0.theta=normalize(q0.theta/RAD); 
q0.kappa=0.0; 
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/*  Point  obstacle  */ 

printf("Input  Coordinates  of  the  pf  (focus).  \n"); 
printf("X=\n"); 
scanf("%lf,&p.x); 
printf("Y=W); 
scanf(n%lF,&p.y); 
} 

FUNCTION:  InputlnitConfigO 

Purpose:         This  procedure  Inputs  the  initial  Configration  of  the  vehicle, 
size  constant  and  step  size. 

void  InputInitConfig(CONHGURATION  &q,double  &sO,double  &DS) 

{ 
/*  Config  of  q  */ 

printf(  "Input  initial  Configuration  of  the  vehicle  q.  \n"); 
printf("X=\n"); 
scanf("%lf',&q.pointx); 
printf("Y=V'); 
scanf("%lf',&q.pointy); 
printf("theta=  \n"); 
scanf("%lf\&q.theta); 
q.theta=normalize(q.theta/RAD); 
printf("kappa=\n"); 
scanf("%lf',&q.kappa); 

/*  Size  constant*/ 

printf("Input  the  size  constant  sOn"); 

printf("sO=V); 

scanf("%lT,&sO); 

/*  DS  */ 

printfflnput  the  step  size  DS.\n"); 

printf("DS=W); 

scanf("%lf',&DS); 

} 

FUNCTION:  GetSizeO 

Purpose:        This  procedure  is  for  a  function  which  computes  the  value  of 
size  of  the  parabola. 

double  GetSize(CONHGURATION  qO  ,POINT  p) 

{ 
return(-(p.x-q0.point.x)*sin(q0.theta)  +  (p.y-q0.point.y)*cos(q0.theta)); 

} 
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FUNCTION:  GetConstants() 

Purpose:         This  procedure  is  for  a  function  which  computes  the  value  of 
constants  k,  a,  b,  and  c. 

void  GetConstants(double  S0,double  &a,double  &b,double  &c) 

{ 
double  ConstK; 

ConstK=1.0/S0; 

a=3.0*ConstK; 

b=3 .0*ConstK*ConstK; 

c=ConstK*ConstK*ConstK; 

} 

FUNCTION:  GetSteeringFunc() 

Purpose:         This  procedure  is  for  a  function  which  computes  the  value  of 
steering  function  dk/ds. 

double  GetSteering(double  a,double  b,double  CONFIGURATION  q, 
CONFIGURATION  qO^OINT  p,double  size) 

{ 
double  kappaPara,phi,deltaKappa,thetaN,thetaDesire, 

deltaTheta,deltaDist,dl  ,d2; 

/*  Calculate  DeltaKappa  */ 
if  (  size  >=  0.0) 
phi  = 

normalize(atan2(q.point.y-p.y,  q.point.x-p.x)  -  (qO.theta-PI/2.0)); 
else 

phi  =  normalize(-atan2(q.point.y-p.y,  q.pointx-p.x)  + 

(qO.theta+PI/2.0)); 
kappaPara  =  cos(phi/2.0)*cos(phi/2.0)*cos(phi/2.0)/size; 
deltaKappa  =  q.kappa  -  kappaPara; 

/*  Calculate  DeltaTheta  */ 

thetaN=((size  >=  0.0)  ?  (qO.theta  -  PI/2.0):(q0.theta  +  PI/2.0)); 

thetaDesire  = 

normalizel((atan2(p.y-q.point.y,  p.x-q.point.x)  +  thetaN)/2.0  -  qO.theta) 

+  qO.theta; 

deltaTheta  =  normalize(q.theta  -  thetaDesire); 

/*  Calculate  DeltaDist  */ 

dl  =  sqrt((p.x-q.point.x)*(p.x-q.point.x)  +  (p.y-q.point.y)*(p.y-q.point.y)); 

d2  =  -(q.point.x-q0.point.x)*sin(q0.theta) 

+  (q.point.y-q0.point.y)*cos(q0.theta); 
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if  (size  >=  0.0) 

deltaDist  =  d2-dl; 
else 

deltaDist  =  d2  +  dl; 

/*  Calculate  Steering  fucnction  =  u  */ 
return(-(a*deltaKappa  +  b*deltaTheta  +  c*deltaDist)); 

} 

FUNCTION:  GetDkappa() 

Purpose:        This  procedure  is  for  a  function  which  computes  the  value  of 
dKappa. 

double  GetDkappa(double  u,double  ds) 

{ 
return(u*ds); 

} 

FUNCTION:  GetKappa() 

Purpose:        This  procedure  is  for  a  function  which  computes  the  value  of 
Kappa. 

void  GetKappa(double  dkappa,CONFIGURATION  &q) 

{ 
q.kappa=q. kappa  +  dkappa; 

} 

FUNCTION:  GetDtheta() 

Purpose:         This  procedure  is  for  a  function  which  computes  the  value  of 
dtheta. 

double  GetDtheta(CONFIGURATION  ql, double  ds) 

{ 
return(ql  .kappa  *  ds); 

} 

FUNCTION:  next() 

Purpose:        This  procedure  is  for  a  function  which  computes  the  next 
configration  of  the  vehicle. 

void  next(double  ds,double  dtheta,double  &s,CONFIGURATION  &q) 

{ 

CONFIGURATION  ql; 
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/*  CONFIGURATION  of  ql  */ 
ql.point.x  =  (1.0  -  dtheta*dtheta/6.0)*ds; 
ql.point.y  =  (0.5  -  dtheta*dtheta/24.0)*dtheta*ds; 
ql.theta  =  dtheta; 

s  =  s  +  ds- 

.    CONFIGURATION  of  q  */ 

q.pointx  =  q.point.x  +  ql.point.x*cos(q.theta)  -  ql.point.y*sin(q.theta); 

q.point.y  =  q.point.y  +  ql. point  x*sin(q.theta)  +  ql.point.y*cos(q.theta); 

q.theta  =  q.theta  +  ql.theta; 

} 

FUNCTION:  Openfile() 

Purpose:         This  procedure  opens  the  output  file. 

******************************************  +  **************■*/ 

void  Openfile(CONHGURATION  q,double  s) 

{ 
fpO  =  fopen("path.dat","w"); 

fpl  =  fopenCpath","  w"); 

fprintf(fpO,"  s  x  y     theta[deg]  kappa  "); 

fprintf(fp0,"    u   deltaKappa  deltaTheta  deltaDistW); 

printf("  s        x        y       theta[deg]  kappaW); 

fprintf(fp0,"%4.4f  %4.4f  %4.4f  %4.4f  %4.4fvn",s,  q.pointx,  q.pointy, 

q.theta*RAD,q.kappa); 
printf("%4.4f  %4.4f  %4.4f  %4.4f  %4.4f\n",  s,  q.pointx,  q.pointy, 

q.theta*RAD,q.kappa); 
fprintf(fpl,"%f  %Ni",  q.pointx,  q.pointy); 
} 

FUNCTION:  PrintfileO 

Purpose:         This  procedure  prints  the  result  to  the  file. 

void  Printfile(CONFIGURA'TION  q,double  s) 

{ 
fprintf(fp0,"%4.4f  %4.4f  %4.4f  %4.4f  %4.4f   ", 

s,  q.pointx,  q.pointy,  q.theta*RAD,q.kappa); 
printf("%4.4f  %4.4f  %4.4f  %4.4f  %4.4f\n", 

s,  q.pointx,  q.pointy,  q.theta*RAD,q.kappa); 
fprintf (fjp  1 ," %f  %f\n",  q.pointx,  q.pointy);    /*  for  gnuplot  */ 
} 

FUNCTION:  mainO 
void  main(void) 
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{ 

CONFIGURATION  qO,q; 
POINT  p; 

double  u;  /*  steering  function  */ 
double  DS,s,sO,a,b,c,size; 
double  dkappa,dtheta; 

InputParabola(qO,p); 
size=GetSize(qO,p) ; 

InputInitConfig(q,sO,DS); 
GetConstants(sO,a,b,c); 

s  =  0.0; 
Openfile(q,s); 

do 

{ 

u  =  GetSteering(a,b,c,q,qO,p,size); 

dkappa  =  GetDkappa(u,DS); 

GetKappa(dkappa,q) ; 

dtheta=GetDtheta(q,DS); 

next(DS,dtheta,s,q); 

Printfile(q,s); 

} 
while(s<=2000.0); 

fclose(fpO); 
fclose(fpl); 
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